#include "HiranipraShared.h"

#ifndef _SHARED_MATH_MATRIX_H_
#define _SHARED_MATH_MATRIX_H_

namespace Hiranipra {
	namespace Math {
#if 0
		//////////////////////////////////////////////////////////////////////////
		// Class: Matrix
		//========================================================================
		// Code for this class taken without permission *grin* from http://www.baskuenen.myweb.nl/
		// Optimized by ben.vanik@gmail.com
		class Matrix
		{
		public:
			union {
				struct {
					Vector RotX;
					float m03;
					Vector RotY;
					float m13;
					Vector RotZ;
					float m23;
					union {
						struct {
							Vector Pos;
						};
						struct {
							float x, y, z;
						};
					};
					float m33;
				};
				float m4x4[4][4];
				float m16[16];
			};

			Matrix( const float x = 0, const float y = 0, const float z = 0 ) :
				Pos(x,y,z), RotX(1,0,0), RotY(0,1,0), RotZ(0,0,1), m33(1)
			{
			}
			Matrix( const Vector& _Pos, const Vector& _RotX = Vector(1,0,0), const Vector& _RotY = Vector(0,1,0), const Vector& _RotZ = Vector(0,0,1) ) :
				Pos(_Pos), RotX(_RotX), RotY(_RotY), RotZ(_RotZ), m33(1)
			{
			}
			Matrix( const Matrix& Matrix ) :
				Pos(Matrix.Pos), RotX(Matrix.RotX), RotY(Matrix.RotY), RotZ(Matrix.RotZ), m33(1)
			{
			}

			//////////////////////////////////////////////////////////////////////////
			// Function: Identify
			//========================================================================
			void __inline Identify()
			{
				// Set everything to 0
				memset( (void*)m4x4, 0, sizeof(float) * 16 );

				// Set diagonals to 1
				m4x4[0][0] = m4x4[1][1] = m4x4[2][2] = m4x4[3][3] = 1;
			}

			//////////////////////////////////////////////////////////////////////////
			// Function: operator =
			//========================================================================
			const Matrix& operator = ( const Matrix& Matrix )
			{
				//Copy the matrix data
				memcpy( (void*)m16, (void*)Matrix.m16, sizeof(float) * 16 );
				
				return( (*this) );
			}
			
			//////////////////////////////////////////////////////////////////////////
			// Function: operator =
			// Notes: Transform equal operator
			//========================================================================
			const Matrix& operator = ( const Vector& _v )
			{
#if defined( X86_MATH )
				__asm
				{
					; Pos	= _v;
					mov ebx, dword ptr [Pos]
					mov ecx, dword ptr [_v]

					mov eax, dword ptr [ecx]
					mov dword ptr [ebx], eax
					mov eax, dword ptr [ecx+4]
					mov dword ptr [ebx+4], eax
					mov eax, dword ptr [ecx+8]
					mov dword ptr [ebx+8], eax
				}
#elif defined( SSE_MATH )
#error SSE Matrix not implemented
#else
                Pos = _v;
#endif
				return( (*this) );
			}

			const bool operator == ( const Matrix& _m ) const
			{
				return( ( Pos == _m.Pos ) &&
						( RotX == _m.RotX ) &&
						( RotY == _m.RotY ) &&
						( RotZ == _m.RotZ ) );
			}
			const bool operator != ( const Matrix& _m ) const
			{
				return( !( (*this) == _m) );
			}

			const Matrix operator + ( const Vector& _v ) const
			{
				return( Matrix( Pos + Vector(_v % RotX, _v % RotY, _v % RotZ), RotX, RotY, RotZ) );
			}
			const Matrix& operator += ( const Vector& _v )
			{
				return( (*this) = (*this) + _v );
			}
			const Matrix operator - ( const Vector& _v ) const
			{
				return( Matrix( Pos - Vector(_v % RotX, _v % RotY, _v % RotZ), RotX, RotY, RotZ) );
			}
			const Matrix& operator -= ( const Vector& _v )
			{
				return( (*this) = (*this) - _v );
			}

			//////////////////////////////////////////////////////////////////////////
			// Function: operator *
			// Notes: Multiply the matrix by another
			//========================================================================
			const Matrix operator * ( const Matrix& _m ) const
			{
				Matrix	mat;

				mat.m4x4[0][0]	= m4x4[0][0] * _m.m4x4[0][0] + m4x4[0][1] * _m.m4x4[1][0] + m4x4[0][2] * _m.m4x4[2][0];
				mat.m4x4[0][1]	= m4x4[0][0] * _m.m4x4[0][1] + m4x4[0][1] * _m.m4x4[1][1] + m4x4[0][2] * _m.m4x4[2][1];
				mat.m4x4[0][2]	= m4x4[0][0] * _m.m4x4[0][2] + m4x4[0][1] * _m.m4x4[1][2] + m4x4[0][2] * _m.m4x4[2][2];
				mat.m4x4[0][3]	= 0;

				mat.m4x4[1][0]	= m4x4[1][0] * _m.m4x4[0][0] + m4x4[1][1] * _m.m4x4[1][0] + m4x4[1][2] * _m.m4x4[2][0];
				mat.m4x4[1][1]	= m4x4[1][0] * _m.m4x4[0][1] + m4x4[1][1] * _m.m4x4[1][1] + m4x4[1][2] * _m.m4x4[2][1];
				mat.m4x4[1][2]	= m4x4[1][0] * _m.m4x4[0][2] + m4x4[1][1] * _m.m4x4[1][2] + m4x4[1][2] * _m.m4x4[2][2];
				mat.m4x4[1][3]	= 0;

				mat.m4x4[2][0]	= m4x4[2][0] * _m.m4x4[0][0] + m4x4[2][1] * _m.m4x4[1][0] + m4x4[2][2] * _m.m4x4[2][0];
				mat.m4x4[2][1]	= m4x4[2][0] * _m.m4x4[0][1] + m4x4[2][1] * _m.m4x4[1][1] + m4x4[2][2] * _m.m4x4[2][1];
				mat.m4x4[2][2]	= m4x4[2][0] * _m.m4x4[0][2] + m4x4[2][1] * _m.m4x4[1][2] + m4x4[2][2] * _m.m4x4[2][2];
				mat.m4x4[2][3]	= 0;

				mat.m4x4[3][0]	= m4x4[3][0] * _m.m4x4[0][0] + m4x4[3][1] * _m.m4x4[1][0] + m4x4[3][2] * _m.m4x4[2][0];
				mat.m4x4[3][1]	= m4x4[3][0] * _m.m4x4[0][1] + m4x4[3][1] * _m.m4x4[1][1] + m4x4[3][2] * _m.m4x4[2][1];
				mat.m4x4[3][2]	= m4x4[3][0] * _m.m4x4[0][2] + m4x4[3][1] * _m.m4x4[1][2] + m4x4[3][2] * _m.m4x4[2][2];
				mat.m4x4[3][3]	= 1;

				return( mat );

			}

			//////////////////////////////////////////////////////////////////////////
			// Function: operator *=
			// Notes: Multiply and equal
			//========================================================================
			const Matrix& operator *= ( const Matrix& _m )
			{
				// (*this) = (*this) * _m

				float	_m4x4[4][4];

				_m4x4[0][0]	= m4x4[0][0] * _m.m4x4[0][0] + m4x4[0][1] * _m.m4x4[1][0] + m4x4[0][2] * _m.m4x4[2][0];
				_m4x4[0][1]	= m4x4[0][0] * _m.m4x4[0][1] + m4x4[0][1] * _m.m4x4[1][1] + m4x4[0][2] * _m.m4x4[2][1];
				_m4x4[0][2]	= m4x4[0][0] * _m.m4x4[0][2] + m4x4[0][1] * _m.m4x4[1][2] + m4x4[0][2] * _m.m4x4[2][2];
				_m4x4[0][3]	= 0;

				_m4x4[1][0]	= m4x4[1][0] * _m.m4x4[0][0] + m4x4[1][1] * _m.m4x4[1][0] + m4x4[1][2] * _m.m4x4[2][0];
				_m4x4[1][1]	= m4x4[1][0] * _m.m4x4[0][1] + m4x4[1][1] * _m.m4x4[1][1] + m4x4[1][2] * _m.m4x4[2][1];
				_m4x4[1][2]	= m4x4[1][0] * _m.m4x4[0][2] + m4x4[1][1] * _m.m4x4[1][2] + m4x4[1][2] * _m.m4x4[2][2];
				_m4x4[1][3]	= 0;

				_m4x4[2][0]	= m4x4[2][0] * _m.m4x4[0][0] + m4x4[2][1] * _m.m4x4[1][0] + m4x4[2][2] * _m.m4x4[2][0];
				_m4x4[2][1]	= m4x4[2][0] * _m.m4x4[0][1] + m4x4[2][1] * _m.m4x4[1][1] + m4x4[2][2] * _m.m4x4[2][1];
				_m4x4[2][2]	= m4x4[2][0] * _m.m4x4[0][2] + m4x4[2][1] * _m.m4x4[1][2] + m4x4[2][2] * _m.m4x4[2][2];
				_m4x4[2][3]	= 0;

				_m4x4[3][0]	= m4x4[3][0] * _m.m4x4[0][0] + m4x4[3][1] * _m.m4x4[1][0] + m4x4[3][2] * _m.m4x4[2][0];
				_m4x4[3][1]	= m4x4[3][0] * _m.m4x4[0][1] + m4x4[3][1] * _m.m4x4[1][1] + m4x4[3][2] * _m.m4x4[2][1];
				_m4x4[3][2]	= m4x4[3][0] * _m.m4x4[0][2] + m4x4[3][1] * _m.m4x4[1][2] + m4x4[3][2] * _m.m4x4[2][2];
				_m4x4[3][3]	= 1;

				memcpy( (void*)this->m4x4, (void*)_m4x4, sizeof(float) * 16 );

				return( (*this) );
			}

			//////////////////////////////////////////////////////////////////////////
			// Function: operator *
			//========================================================================
			const Matrix operator * ( const Vector& _v ) const
			{
				return( (Matrix)Matrix(Pos * _v, RotX, RotY, RotZ) );
			}

			//////////////////////////////////////////////////////////////////////////
			// Function: operator !
			// Notes: Inverse matrix
			//========================================================================
			const Matrix FAST_CALL operator ! () const
			{
				Matrix Matrix( Vector(0,0,0),
								Vector(RotX.x, RotY.x, RotZ.x),
								Vector(RotX.y, RotY.y, RotZ.y),
								Vector(RotX.z, RotY.z, RotZ.z)	);
				return( Matrix - Pos );
			}

			//////////////////////////////////////////////////////////////////////////
			// Function: RotateX
			//========================================================================
			const Matrix FAST_CALL RotateX( float angle )
			{
				// Calculate sin/cos
				float sin0	= (float)sin( angle );
				float cos0	= (float)cos( angle );

				// Setup matrix
				this->m4x4[1][1]	= ( cos0 );
				this->m4x4[1][2]	= ( sin0 );
				this->m4x4[2][1]	= (-sin0 );
				this->m4x4[2][2]	= ( cos0 );

				return( (*this) );
			}

			//////////////////////////////////////////////////////////////////////////
			// Function: RotateY
			//========================================================================
			const Matrix FAST_CALL RotateY( float angle )
			{
				// Calculate sin/cos
				float sin0	= (float)sin( angle );
				float cos0	= (float)cos( angle );

				// Setup matrix
				m4x4[0][0]	= ( cos0 );
				m4x4[0][2]	= (-sin0 );
				m4x4[2][0]	= ( sin0 );
				m4x4[2][2]	= ( cos0 );

				return( (*this) );
			}

			//////////////////////////////////////////////////////////////////////////
			// Function: RotateZ
			//========================================================================
			const Matrix FAST_CALL RotateZ( float angle )
			{
				// Calculate sin/cos
				float sin0	= (float)sin( angle );
				float cos0	= (float)cos( angle );

				// Setup matrix
				m4x4[0][0]	= ( cos0 );
				m4x4[0][1]	= ( sin0 );
				m4x4[1][0]	= (-sin0 );
				m4x4[1][1]	= ( cos0 );

				return( (*this) );
			}

			//////////////////////////////////////////////////////////////////////////
			// Function: Rotate
			//========================================================================
			void Rotate( float fX, float fY, float fZ )
			{
				float	sin0, sin1;
				float	cos0, cos1;
				int		product	= 0;
				Matrix	mat;
				float	rotateX[4][4],
						rotateY[4][4],
						rotateZ[4][4],
						temp[4][4];

				// Identiy the matrix
				mat.Identify();

				// Figure out what we need to do
				if( x )
					product	+= 4;
				if( y )
					product	+= 2;
				if( z )
					product	+= 1;

				// Compute the matrix
				switch( product )
				{
				case( 1 ): // Final matrix = z

					// Calculate sin/cos
					sin0	= (float)sin(z);
					cos0	= (float)cos(z);

					// Setup matrix
					mat.m4x4[0][0]	= ( cos0 );
					mat.m4x4[0][1]	= ( sin0 );
					mat.m4x4[1][0]	= (-sin0 );
					mat.m4x4[1][1]	= ( cos0 );
					
					break;

				case( 2 ): // Final matrix = y

					// Calculate sin/cos
					sin0	= (float)sin(y);
					cos0	= (float)cos(y);

					// Setup matrix
					mat.m4x4[0][0]	= ( cos0 );
					mat.m4x4[0][2]	= (-sin0 );
					mat.m4x4[2][0]	= ( sin0 );
					mat.m4x4[2][2]	= ( cos0 );
					
					break;

				case( 3 ): // Final matrix = y * z

					// Calculate sin/cos
					sin0	= (float)sin(y);
					cos0	= (float)cos(y);
					sin1	= (float)sin(z);
					cos1	= (float)cos(z);

					mat.m4x4[0][0]	= cos0 * cos1;
					mat.m4x4[0][1]	= cos0 * sin1;
					mat.m4x4[0][2]	= -sin0;

					mat.m4x4[1][0]	= -sin1;
					mat.m4x4[1][1]	= cos1;
					
					mat.m4x4[2][0]	= sin0 * cos1;
					mat.m4x4[2][1]	= sin0 * sin1;
					mat.m4x4[2][2]	= cos0;

					break;

				case( 4 ): // Final matrix = x

					// Calculate sin/cos
					sin0	= (float)sin(x);
					cos0	= (float)cos(x);

					// Setup matrix
					mat.m4x4[1][1]	= ( cos0 );
					mat.m4x4[1][2]	= ( sin0 );
					mat.m4x4[2][1]	= (-sin0 );
					mat.m4x4[2][2]	= ( cos0 );
				
					break;

				case( 5 ): // Final matrix = x * z

					// Calculate sin/cos
					sin0	= (float)sin(x);
					cos0	= (float)cos(x);
					sin1	= (float)sin(z);
					cos1	= (float)cos(z);

					mat.m4x4[0][0]	= cos1;
					mat.m4x4[0][1]	= sin1;

					mat.m4x4[1][0]	= -cos0 * sin1;
					mat.m4x4[1][1]	= cos0 * cos1;
					mat.m4x4[1][2]	= sin0;
					
					mat.m4x4[2][0]	= sin0 * sin1;
					mat.m4x4[2][1]	= -sin0 * cos1;
					mat.m4x4[2][2]	= cos0;

					break;

				case( 6 ): // Final matrix = x * y

					// Calculate sin/cos
					sin0	= (float)sin(x);
					cos0	= (float)cos(x);
					sin1	= (float)sin(y);
					cos1	= (float)cos(y);

					mat.m4x4[0][0]	= cos1;
					mat.m4x4[0][2]	= -sin1;

					mat.m4x4[1][0]	= sin0 * sin1;
					mat.m4x4[1][1]	= cos0;
					mat.m4x4[1][2]	= sin0 * cos1;
					
					mat.m4x4[2][0]	= cos0 * sin1;
					mat.m4x4[2][1]	= -sin0;
					mat.m4x4[2][2]	= cos0 * cos1;

					break;

				case( 7 ): // Final matrix = x * y * z

					// x
					Mat_Identity_4x4( (float**)rotateX );
					sin0			= (float)sin(x);
					cos0			= (float)cos(x);
					rotateX[1][1]	= ( cos0 );
					rotateX[1][2]	= ( sin0 );
					rotateX[2][1]	= (-sin0 );
					rotateX[2][2]	= ( cos0 );

					// y
					Mat_Identity_4x4( (float**)rotateY );
					sin0			= (float)sin(y);
					cos0			= (float)cos(y);
					rotateY[0][0]	= ( cos0 );
					rotateY[0][2]	= (-sin0 );
					rotateY[2][0]	= ( sin0 );
					rotateY[2][2]	= ( cos0 );

					// z
					Mat_Identity_4x4( (float**)rotateZ );
					sin0			= (float)sin(z);
					cos0			= (float)cos(z);
					rotateZ[0][0]	= ( cos0 );
					rotateZ[0][1]	= ( sin0 );
					rotateZ[1][0]	= (-sin0 );
					rotateZ[1][1]	= ( cos0 );

					Mat_Mul_4x4_4x4( (float**)rotateX, (float**)rotateY, (float**)temp );
					Mat_Mul_4x4_4x4( (float**)temp, (float**)rotateZ, (float**)mat.m4x4 );

					break;

				};
			}

			const Vector FAST_CALL Transform( const Vector& _v )
			{
				return( Vector(_v % RotX, _v % RotY, _v % RotZ) + Pos );
			}


		private:
			void Mat_Identity_4x4(float **fMatrix)
			{
				// Set everything to 0
				memset( (void*)&fMatrix[0][0], 0, sizeof(float) * 16 );

				// Set diagonals to 1
				fMatrix[0][0] = fMatrix[1][1] = fMatrix[2][2] = fMatrix[3][3] = 1;
			}

			void Mat_Mul_4x4_4x4(float **fA, float **fB, float **fDest)
			{
				fDest[0][0]	= fA[0][0] * fB[0][0] + fA[0][1] * fB[1][0] + fA[0][2] * fB[2][0];
				fDest[0][1]	= fA[0][0] * fB[0][1] + fA[0][1] * fB[1][1] + fA[0][2] * fB[2][1];
				fDest[0][2]	= fA[0][0] * fB[0][2] + fA[0][1] * fB[1][2] + fA[0][2] * fB[2][2];
				fDest[0][3]	= 0;

				fDest[1][0]	= fA[1][0] * fB[0][0] + fA[1][1] * fB[1][0] + fA[1][2] * fB[2][0];
				fDest[1][1]	= fA[1][0] * fB[0][1] + fA[1][1] * fB[1][1] + fA[1][2] * fB[2][1];
				fDest[1][2]	= fA[1][0] * fB[0][2] + fA[1][1] * fB[1][2] + fA[1][2] * fB[2][2];
				fDest[1][3]	= 0;

				fDest[2][0]	= fA[2][0] * fB[0][0] + fA[2][1] * fB[1][0] + fA[2][2] * fB[2][0];
				fDest[2][1]	= fA[2][0] * fB[0][1] + fA[2][1] * fB[1][1] + fA[2][2] * fB[2][1];
				fDest[2][2]	= fA[2][0] * fB[0][2] + fA[2][1] * fB[1][2] + fA[2][2] * fB[2][2];
				fDest[2][3]	= 0;

				fDest[3][0]	= fA[3][0] * fB[0][0] + fA[3][1] * fB[1][0] + fA[3][2] * fB[2][0];
				fDest[3][1]	= fA[3][0] * fB[0][1] + fA[3][1] * fB[1][1] + fA[3][2] * fB[2][1];
				fDest[3][2]	= fA[3][0] * fB[0][2] + fA[3][1] * fB[1][2] + fA[3][2] * fB[2][2];
				fDest[3][3]	= 1;
			}
		};
#endif
	}
}

#endif // _SHARED_MATH_MATRIX_H_
